//////////////////////////////
// trame
void send_start_trame(void)
{
  Serial1.print("#");
}
void send_end_trame(void)
{
  Serial1.println("$");
}
void send_data(char *str)
{
  Serial1.print(str);
}
void send_data(unsigned long ul)
{
  Serial1.print(ul);
}
void send_data(int t)
{
  Serial1.print(t);
}
void send_data(short s)
{
  Serial1.print(s);
}


//////////////////////////////
// error
void send_error(String err)
{
  char e[256];
  err.toCharArray(e, sizeof(e));
  send_error(e);
}
void send_error(char *error)
{
  send_start_trame();
  send_data("error:");
  send_data(error);
  send_end_trame();
}

//////////////////////////////
// message
void send_message(String msg)
{
  char m[256];
  msg.toCharArray(m, sizeof(m));
  send_message(m);
}
void send_message(char *msg)
{
  send_start_trame();
  send_data("message:");
  send_data(msg);
  send_end_trame();
}

//////////////////////////////
// version
void send_version(void)
{
  send_start_trame();
  send_data("version:");
  send_data("name=");
  send_data(_ROBOT_NAME_);
  send_data(",version=");
  send_data(_ROBOT_VERSION_);
  send_end_trame();
}

void send_ready(boolean bReady)
{
  send_start_trame();
  send_data("ready:");
  send_data("status=");
  if(bReady) send_data("ok");
  else send_data("nok");
  send_end_trame();
}

void send_ping()
{
  send_start_trame();
  send_data("ready:");
  send_data("status=ping");
  send_end_trame();
}

void send_tick(unsigned long slicetime)
{
  send_start_trame();
  send_data("tick:");
  send_data("slicetime=");
  send_data(slicetime);
  send_end_trame();
}

void send_range(short telemetre)
{
  send_start_trame();
  send_data("range:");
  send_data("front=");
  send_data(telemetre);
  send_end_trame();
}

void send_servos_position()
{
  send_start_trame();
  send_data("servo_position:");
  for(int i=0; i<N_SERVOS; i++)
  {
    if(i>0) send_data(",");
    send_data(servo_ind[i]);
    send_data("=");
    send_data(servo_pos[i]);
  }
  send_end_trame();
}

